#ifndef __CONTROL_H
#define __CONTROL_H

#define CLAMP(x, lower, upper)	(x >= upper ? upper : (x <= lower ? lower : x))

typedef struct
{
	float Target;
	float NowIS;
	
	float Err;
	float Err_last;
	float Err_sum;
	
	float Kp;
	float Ki;
	float Kd;
	
	int Max;
	int Min;

	float P_Out;
	float I_Out;
	float D_Out;
	float PID_Out;
	
	float DeadBand;
	float IntegralLimit;
}PID;

typedef struct
{
    int wheelSpeed;
}WheelInfo;

double FabsD(double x);
void motorSetSpeed(int goalSpeed, int ID);
void wheelsRun(void);
void carSetSpeed(float VX_W, float VY_W, float VZ_W);

#endif
